//！ 三维空间算法
const lbmath = @import("./matrix/math/lbmath.zig");
// const zlbmath = @import("zlbmath_lib");
// const lbmath = zlbmath.lbmath;
const std = @import("std");
const Math_Compute_Abandon = lbmath.Math_Compute_Abandon;
const dprint = std.debug.print;
const testing = std.testing;
const math = std.math;
const lbmatrix = @import("./matrix/lbmatrix.zig");
// array literal
// const message = [_]u8{ 'h', 'e', 'l', 'l', 'o' };

// // alternative initialization using result location
// const alt_message: [5]u8 = .{ 'h', 'e', 'l', 'l', 'o' };

// comptime {
//     assert(mem.eql(u8, &message, &alt_message));
// }
test compute_plane_normal_of_jordan_curve {
    const vs = [4][3]f64{
        [3]f64{ 1, 2, 3 },
        [3]f64{ 4, 5, 6 },
        [3]f64{ 1, 2, -3 },
        [3]f64{ -4, -5, -1 },
    };
    const re = compute_plane_normal_of_jordan_curve(vs);
    dprint(" compute plane normal jordan curve {any}\n ", .{re});
}

fn compute_plane_normal_of_jordan_curve(vs: anytype) [3]@TypeOf(vs[0][0]) {
    // std.mem.zeroInit(comptime T: type, init: anytype)
    // @memset(dest, elem);
    std.debug.assert(vs.len > 1);
    const T = @TypeOf(vs[0][0]);
    var re = [3]T{ 0, 0, 0 };
    // var n: [3]T = [_]T{0} ** 3;
    for (0..vs.len) |i| {
        const j = (i + vs.len - 1) % vs.len;
        const k = (i + 1) % vs.len;

        var a: [3]T = .{ vs[i][0] - vs[j][0], vs[i][1] - vs[j][1], vs[i][2] - vs[j][2] };
        var b: [3]T = .{ vs[k][0] - vs[i][0], vs[k][1] - vs[i][1], vs[k][2] - vs[i][2] };

        lbmath.normalize(&a) catch continue;
        lbmath.normalize(&b) catch continue;
        var n = lbmath.out_product(a[0..3], b[0..3]);
        const temp = std.math.acos(lbmath.inner_product(&a, &b));
        lbmath.normalize(&n) catch continue;
        re[0] += temp * n[0];
        re[1] += temp * n[1];
        re[2] += temp * n[2];
    }
    return re;
}
test "get_line_equation_from_two_points normal case" {
    const p1 = [3]f32{ 0.0, 0.0, 0.0 };
    const p2 = [3]f32{ 1.0, 1.0, 0.0 };
    var re: [6]f32 = undefined;

    const result = get_line_equation_from_two_points(p1, &p2, &re);

    try testing.expect(result == 1);
    // 测试方向向量归一化结果
    try testing.expectApproxEqAbs(@as(f32, 0.707), re[0], 0.01);
    try testing.expectApproxEqAbs(@as(f32, 0.707), re[1], 0.01);
    try testing.expectApproxEqAbs(@as(f32, 0.0), re[2], 0.01);
    // 测试起点
    try testing.expect(re[3] == 0.0);
    try testing.expect(re[4] == 0.0);
    try testing.expect(re[5] == 0.0);
}
///返回一个经过两点的线程方程，不管两点是否一样，是否有无数个方程
/// @return 1(只有一个方程) 0（无数个方程）
fn get_line_equation_from_two_points(p1: anytype, p2: *const [3]@TypeOf(p1[0]), re: *[6]@TypeOf(p1[0])) i32 {
    re[3] = p1[0];
    re[4] = p1[1];
    re[5] = p1[2];
    re[0] = p2[0] - p1[0];
    re[1] = p2[1] - p1[1];
    re[2] = p2[2] - p1[2];
    lbmath.normalize(re[0..3]) catch {
        re[0] = 1;
        re[1] = 0;
        re[2] = 0;
        return 0;
    };
    return 1;
}

// test get_plane_equation_of_triangular_plane
// {

// }

// test "test array copy" {
//     dprint("test array copy\n");
// }
// 测试三角形平面：三点确定一个平面
test "get_plane_equation_of_triangular_plane - valid triangle" {
    const T = f64;
    var tr1: [3]T = [3]T{ 0, 0, 0 };
    var tr2: [3]T = [3]T{ 1, 0, 0 };
    var tr3: [3]T = [3]T{ 0, 1, 0 };

    var re: [4]T = undefined;

    const result = get_plane_equation_of_triangular_plane(&tr1, &tr2, &tr3, &re);

    try testing.expectEqual(@as(i32, 1), result);
    try testing.expectApproxEqAbs(@as(T, 0), re[0], 1e-10); // a
    try testing.expectApproxEqAbs(@as(T, 0), re[1], 1e-10); // b
    try testing.expectApproxEqAbs(@as(T, 1), re[2], 1e-10); // c
    try testing.expectApproxEqAbs(@as(T, 0), re[3], 1e-10); // d

    // 验证三个点满足 ax+by+cz+d = 0
    try testing.expectApproxEqAbs(re[0] * tr1[0] + re[1] * tr1[1] + re[2] * tr1[2] + re[3], 0, 1e-10);
    try testing.expectApproxEqAbs(re[0] * tr2[0] + re[1] * tr2[1] + re[2] * tr2[2] + re[3], 0, 1e-10);
    try testing.expectApproxEqAbs(re[0] * tr3[0] + re[1] * tr3[1] + re[2] * tr3[2] + re[3], 0, 1e-10);

    // 验证法向量已归一化
    const norm = @sqrt(re[0] * re[0] + re[1] * re[1] + re[2] * re[2]);
    try testing.expectApproxEqAbs(@as(T, 1.0), norm, 1e-10);
}

// 测试多边形平面：正方形在 xy 平面
// test "get_plane_equation_of_polygon - square in xy-plane" {
//     const T = f64;
//     var poly = [4][3]T{
//         [3]T{ 0, 0, 0 },
//         [3]T{ 1, 0, 0 },
//         [3]T{ 1, 1, 0 },
//         [3]T{ 0, 1, 0 },
//     };

//     var re: [4]T = undefined;

//     const result = get_plane_equation_of_polygon(&poly, &re);

//     try testing.expectEqual(@as(u32, 1), result);
//     try testing.expectApproxEqAbs(@as(T, 0), re[0], 1e-10); // a
//     try testing.expectApproxEqAbs(@as(T, 0), re[1], 1e-10); // b
//     try testing.expectApproxEqAbs(@as(T, 1), re[2], 1e-10); // c
//     try testing.expectApproxEqAbs(@as(T, 0), re[3], 1e-10); // d

//     // 验证所有点在平面上
//     for (poly) |point| {
//         const value = re[0] * point[0] + re[1] * point[1] + re[2] * point[2] + re[3];
//         try testing.expectApproxEqAbs(@as(T, 0), value, 1e-10);
//     }

//     // 验证法向量归一化
//     const norm = @sqrt(re[0] * re[0] + re[1] * re[1] + re[2] * re[2]);
//     try testing.expectApproxEqAbs(@as(T, 1.0), norm, 1e-10);
// }

// 测试退化情况：三点共线 → 无法确定平面
// test "get_plane_equation_of_triangular_plane - collinear points" {
//     const T = f64;
//     var tr1: [3]T = [3]T{ 0, 0, 0 };
//     var tr2: [3]T = [3]T{ 1, 0, 0 };
//     var tr3: [3]T = [3]T{ 2, 0, 0 }; // 共线

//     var re: [4]T = undefined;

//     const result = get_plane_equation_of_triangular_plane(&tr1, &tr2, &tr3, &re);

//     try testing.expectEqual(@as(u32, 0), result); // 应失败
// }

// 测试退化多边形：所有点重合
// test "get_plane_equation_of_polygon - degenerate polygon" {
//     const T = f64;
//     var poly = [3][3]T{
//         [3]T{ 0, 0, 0 },
//         [3]T{ 0, 0, 0 },
//         [3]T{ 0, 0, 0 },
//     };

//     var re: [4]T = undefined;

//     const result = get_plane_equation_of_polygon(&poly, &re);

//     try testing.expectEqual(@as(u32, 0), result);
// }
///
/// return (a,b,c,d) where ax+by+cz+d=0 ,(a,b,c) is normalized vector
/// 返回经过三点的平面，不管是否有无数个平面
/// 法向归一化了
/// @return 1(只有一个平面) 0（无数个平面)
///
pub fn get_plane_equation_of_triangular_plane(tr1: anytype, tr2: *const [3]@TypeOf(tr1[0]), tr3: *const [3]@TypeOf(tr1[0]), re: *[4]@TypeOf(tr1[0])) i32 {
    const T = @TypeOf(tr1[0]);
    var a: [3]T = .{ tr2[0] - tr1[0], tr2[1] - tr1[1], tr2[2] - tr1[2] };
    var b: [3]T = .{ tr3[0] - tr2[0], tr3[1] - tr2[1], tr3[2] - tr2[2] };
    var n = lbmath.out_product(a[0..3], b[0..3]);

    re[0] = 1;
    re[1] = 0;
    re[2] = 0;
    re[3] = 0;
    lbmath.normalize(&n) catch return 0;
    re[0] = n[0];
    re[1] = n[1];
    re[2] = n[2];
    re[3] = -(re[0] * tr1[0] + re[1] * tr1[1] + re[2] * tr1[2]);
    return 1;
}

// 返回经过多边形的平面，不管是否有无数个平面,
// 法向归一化了
// @return 1(只有一个平面) 0（无数个平面）
//
fn get_plane_equation_of_polygon(poly: anytype, re: *[4]@TypeOf(poly[0][0])) i32 {
    // var n:[3]T=.{0,0,0};
    // var n = [3]T{0} ** 3;
    var res = 0;
    var n = compute_plane_normal_of_jordan_curve(poly);
    re[0] = 1.0;
    re[1] = 0.0;
    re[2] = 0.0;
    re[3] = 0.0;
    if (lbmath.normalize(&n)) {
        re[0] = n[0];
        re[1] = n[1];
        re[2] = n[2];
        res = 1;
    } else |_| {}
    re[3] = -(re[0] * poly[0][0] + re[1] * poly[0][1] + re[2] * poly[0][2]);

    return res;
}

/// 验证方向向量是否与两平面法向量垂直
fn validate_direction(
    p1: *[4]f32,
    p2: *[4]f32,
    dir: *[3]f32,
) !void {
    const dot1 = lbmath.inner_product(dir, p1[0..3]);
    const dot2 = lbmath.inner_product(dir, p2[0..3]);
    try testing.expect(math.approxEqAbs(f32, dot1, 0, 1e-5));
    try testing.expect(math.approxEqAbs(f32, dot2, 0, 1e-5));
    // math.approxEqAbs(comptime T: type, x: T, y: T, tolerance: T)
}

/// 验证点是否在两个平面上
fn validate_point(
    p1: *[4]f32,
    p2: *[4]f32,
    point: *[3]f32,
) !void {
    const val1 = lbmath.inner_product(point, p1[0..3]) + p1[3];
    const val2 = lbmath.inner_product(point, p2[0..3]) + p2[3];
    try testing.expect(math.approxEqAbs(f32, val1, 0, 1e-5));
    try testing.expect(math.approxEqAbs(f32, val2, 0, 1e-5));
}

/// 验证向量是否为单位向量
fn validate_normalized(vec: *[3]f32) !void {
    const len = @sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
    try testing.expect(math.approxEqAbs(f32, len, 1.0, 1e-5));
}

test "正常情况: xy平面与xz平面交线 (x轴)" {
    // xy平面: z = 0  -> [0,0,1,0]
    // xz平面: y = 0  -> [0,1,0,0]
    var p1 = [4]f32{ 0, 0, 1, 0 };
    var p2 = [4]f32{ 0, 1, 0, 0 };
    var re = [_]f32{0} ** 6;

    const res = get_line_equation_from_2planes(&p1, &p2, &re);
    try testing.expectEqual(@as(i32, 1), res);

    // 验证方向向量 (应为[1,0,0])
    try validate_direction(&p1, &p2, re[0..3]);
    // try testing.expect(math.approxEq(f32, re[0], 1.0, EPS));
    // try testing.expect(math.approxEq(f32, re[1], 0.0, EPS));
    // try testing.expect(math.approxEq(f32, re[2], 0.0, EPS));
    try validate_normalized(re[0..3]);

    // 验证点坐标 (应为原点[0,0,0])
    try validate_point(&p1, &p2, re[3..6]);
    // try testing.expect(math.approxEq(f32, re[3], 0.0, EPS));
    // try testing.expect(math.approxEq(f32, re[4], 0.0, EPS));
    // try testing.expect(math.approxEq(f32, re[5], 0.0, EPS));
}

test "平行但不重合: z=0 与 z=1" {
    // z=0 -> [0,0,1,0]
    // z=1 -> [0,0,1,-1]
    var p1 = [4]f32{ 0, 0, 1, 0 };
    var p2 = [4]f32{ 0, 0, 1, -1 };
    var re = [_]f32{0} ** 6;

    const res = get_line_equation_from_2planes(&p1, &p2, &re);
    try testing.expectEqual(@as(i32, 0), res);
    std.debug.print("re line:{any}***************\n", .{re});
    // 验证方向向量 (应为[0,0,1])
    // try testing.expect(math.approxEq(f32, re[0], 0.0, EPS));
    // try testing.expect(math.approxEq(f32, re[1], 0.0, EPS));
    // try testing.expect(math.approxEq(f32, re[2], 1.0, EPS));
    // try validate_normalized(re[0..3]); // 关键：验证归一化

}
//return 0 有无数个直线
// return 1 只有一个直线
fn get_line_equation_from_2planes(p1: anytype, p2: *const [4]@TypeOf(p1[0]), re: *[6]@TypeOf(p1[0])) i32 {
    @memset(re, 0);
    var n = lbmath.out_product(p1[0..3], p2[0..3]);
    lbmath.normalize(&n) catch {
        re[0] = 1.0;
        re[1] = 0.0;
        re[2] = 0.0;
        // 修复：显式指定3维切片，避免越界
        const fac = lbmath.inner_product(re[0..3], p1[0..3]); // 关键修正！
        re[0] -= fac * p1[0];
        re[1] -= fac * p1[1];
        re[2] -= fac * p1[2];
        lbmath.normalize(re[0..3]) catch {
            re[0] = 0;
            re[1] = 1.0;
            re[2] = 0;
        };
        re[3] = -(p1[3] * p1[0] + p2[3] * p2[0]) * 0.5;
        re[4] = -(p1[3] * p1[1] + p2[3] * p2[1]) * 0.5;
        re[5] = -(p1[3] * p1[2] + p2[3] * p2[2]) * 0.5;
        return 0;
    };
    const T = @TypeOf(p1[0]);
    const fac = lbmath.inner_product(p1, p2);
    const fac1 = 1 / (fac * fac - 1);
    const mat: [3][3]T = [3][3]T{
        [3]T{ fac1 * (fac * p2[0] - p1[0]), fac1 * (fac * p2[1] - p1[1]), fac1 * (fac * p2[2] - p1[2]) },
        [3]T{ fac1 * (fac * p1[0] - p2[0]), fac1 * (fac * p1[1] - p2[1]), fac1 * (fac * p1[2] - p2[2]) },
        [3]T{ n[0], n[1], n[2] },
    };
    const b: [3]T = [3]T{ -p1[3], -p2[3], 0 };
    // double * re=(double*)malloc(sizeof(double)*6);
    for (0..3) |i| {
        re[i + 3] = 0;
        for (0..3) |j| {
            re[i + 3] += b[j] * mat[j][i];
        }
    }

    re[0] = n[0];
    re[1] = n[1];
    re[2] = n[2];
    return 1;
}
// 计算两直线l上离l1最近的点，l1上离l最近的点
// t1*n1+p1
// t2*n2+p2
// return 0 有无数对点
// return 1 只有一对点
fn line_point_nearest_2_line(
    l: anytype,
    l1: *[6]@TypeOf(l[0]),
    t1: *@TypeOf(l[0]),
    t2: *@TypeOf(l[0]),
) i32 {
    // solve min |Ax+b|

    const T = @TypeOf(l[0]);
    const b = [3]T{ l[3] - l1[3], l[4] - l1[4], l[5] - l1[5] };
    const A = [3][2]T{
        [2]T{ l[0], -l1[0] },
        [2]T{ l[1], -l1[1] },
        [2]T{ l[2], -l1[2] },
    };
    var B: [2][2]T = [_][2]T{[2]T{ 0, 0 }} ** 2;

    // 计算 A^T * A
    for (0..2) |i| {
        for (0..i + 1) |j| {
            for (0..3) |k| {
                B[i][j] += A[k][i] * A[k][j];
            }
            B[j][i] = B[i][j];
        }
    }
    const det: T = B[0][0] * B[1][1] - B[0][1] * B[1][0];
    if (lbmath.approxEqAbs(T, det, 0, null)) {
        t1.* = 0;
        t2.* = l1[0] * (l[3] - l1[3]) +
            l1[1] * (l[4] - l1[4]) +
            l1[2] * (l[5] - l1[5]);
        // t2.* = -(A[0][0] * b[0] + A[1][0] * b[1] + A[2][0] * b[2]) / B[0][1];
        return 0;
    }
    const B_inverse: [2][2]T = [2][2]T{
        [2]T{ B[1][1], -B[0][1] },
        [2]T{ -B[1][0], B[0][0] },
    };
    const Ab: [2]T = [2]T{
        A[0][0] * b[0] + A[1][0] * b[1] + A[2][0] * b[2],
        A[0][1] * b[0] + A[1][1] * b[1] + A[2][1] * b[2],
    };
    t1.* = -(Ab[0] * B_inverse[0][0] + Ab[1] * B_inverse[0][1]) / det;
    t2.* = -(Ab[0] * B_inverse[1][0] + Ab[1] * B_inverse[1][1]) / det;
    return 1;
}
// l: n*t+p
// return t
pub fn line_point_nearest_2_point(l: anytype, p: *const [3]@TypeOf(l[0])) @TypeOf(l[0]) {
    const T = @TypeOf(l[0]);
    const fac: T = lbmath.inner_product(l[0..3], l[0..3]);
    if (lbmath.approxEqAbs(T, fac, 0, null)) {
        return 0;
    }
    return ((p[0] - l[3]) * l[0] + (p[1] - l[4]) * l[1] + (p[2] - l[5]) * l[2]) / fac;
}

fn Sqrt(comptime T: type) type {
    return switch (@typeInfo(T)) {
        .int => |int| @Type(.{ .int = .{ .signedness = .unsigned, .bits = (int.bits + 1) / 2 } }),
        else => T,
    };
}
// @ l: the normalized line n1*t1+p1
//
fn distance_from_point_to_line(p: anytype, l: *[6]@TypeOf(p[0])) Sqrt(@TypeOf(p[0])) {
    const T = @TypeOf(p[0]);
    const a: [3]T = [3]T{ p[0] - l[3], p[1] - l[4], p[2] - l[5] };
    const dis: T = a[0] * l[0] + a[1] * l[1] + a[2] * l[2];
    const dis1: T = a[0] * a[0] + a[1] * a[1] + a[2] * a[2];

    return lbmath.safe_sqrt(dis1 - dis * dis);
}
// 这里应该抽象出来点到n维单形最近点的算法。
// back_dim 应该大于等于len-1
// len>=1
// @ return 1：这个最近点就是单形所在线性空间的全局最近点
// 0:这个最近点不是单形所在线性空间的全局最近点
// -1: 计算失败
pub fn compute_neareat_point2simplex(comptime T: type, comptime back_dim: usize, comptime len: usize, ps: *const [len]*const [back_dim]T, p: *[back_dim]T, lambda: *[len]T) i32 {
    // int re=1;
    if (len < 1 or back_dim < len - 1) {
        return -1;
    }
    if (len == 1) {
        lambda[0] = 1;
        return 1;
    } else if (len == 2) {
        var fac1: T = 0;
        var fac2: T = 0;
        // for(int i=0;i<back_dim;i++)
        for (0..back_dim) |i| {
            fac1 += (ps[1][i] - ps[0][i]) * (ps[1][i] - ps[0][i]);
            fac2 += (ps[1][i] - ps[0][i]) * (p[i] - ps[0][i]);
        }
        lambda[1] = fac2 / fac1;
        var re: u32 = 0;
        if (lambda[1] > 1) {
            lambda[1] = 1;
        } else if (lambda[1] < 0) {
            lambda[1] = 0;
        } else {
            re = 1;
        }
        lambda[0] = 1 - lambda[1];
        return re;
    }

    @memset(lambda, 0);

    var mat = lbmatrix.LBMatrix(T, len - 1, len - 1).init(0);
    // LB_Matrix mat;
    // lb_matrix_init_double(&mat,len-1,len-1);
    // for(int i=0;i<len-1;i++)
    for (0..len - 1) |i| {
        // for(int j=i;j<len-1;j++)
        for (i..len - 1) |j| {
            // *((double*)(mat.element(&mat,i,j)))=0;
            mat.data[i][j] = 0;
            // for(int k=0;k<back_dim;k++)
            for (0..back_dim) |k| {
                mat.data[i][j] += (ps[i + 1][k] - ps[0][k]) * (ps[j + 1][k] - ps[0][k]);
            }
            mat.data[j][i] = mat.data[i][j];
        }
    }
    const mati = mat.inverse() catch return -1;
    // LB_Matrix* mati=mat.inverse(&mat);
    // free(mat.data);

    // if(mati==NULL){return -1;}
    // double* y=(double*)malloc(sizeof(double)*(len-1));
    var y: [len - 1]T = [_]T{0} ** (len - 1);
    // memset(y, 0, sizeof(double)*(len-1));
    // for(int i=0;i<len-1;i++ )
    for (0..len - 1) |i| {
        y[i] = 0;
        // for(int k=0;k<back_dim;k++)
        for (0..back_dim) |k| {
            y[i] += (p[k] - ps[0][k]) * (ps[i + 1][k] - ps[0][k]);
        }
    }
    // double* data=(double*)(mati->data);
    lambda[0] = 0;
    // for(int i=0;i<len-1;i++)
    for (0..len - 1) |i| {
        lambda[i + 1] = 0;
        // for(int k=0;k<len-1;k++)
        for (0..len - 1) |k| {
            lambda[i + 1] += mati.data[i][k] * y[k];
        }
        lambda[0] += lambda[i + 1];
    }
    lambda[0] = 1 - lambda[0];
    // free(y);

    // lb_matrix_free(mati);
    var flag: u32 = 1;
    // for(int i=0;i<len;i++)
    for (0..len) |i| {
        if (lambda[i] < 0) {
            flag = 0;
            break;
        }
    }
    if (flag == 1) {
        return 1;
    }

    // double* lambda1=(double*)malloc(sizeof(double)*(len-1));
    // double** ps1=(double**)malloc(sizeof(double*)*(len-1));
    var lambda1 = [_]T{0} ** (len - 1);
    var ps1: [len - 1]*const [back_dim]T = undefined;
    var dis: T = -1;
    // for(int i=0;i<len;i++)
    for (0..len) |i| {
        @memset(&lambda1, 0);
        var k: u32 = 0;
        // for(int j=0;j<len;j++)
        for (0..len) |j| {
            if (j != i) {
                ps1[k] = ps[j];
                k += 1;
            }
        }

        const temp_re = compute_neareat_point2simplex(T, back_dim, len - 1, &ps1, p, &lambda1);
        if (temp_re == -1) {
            continue;
        }
        var temp_dis: T = 0;
        // for(int j=0;j<back_dim;j++)
        for (0..back_dim) |j| {
            var tempd = p[j];
            // for(k=0;k<len-1;k++)
            k = 0;
            while (k < len - 1) : (k += 1) {
                tempd = tempd - lambda1[k] * ps1[k][j];
            }
            temp_dis += tempd * tempd;
        }

        if (temp_re == 1 or dis < 0 or temp_dis < dis) {
            lambda[i] = 0;
            dis = temp_dis;
            k = 0;
            // for(int j=0;j<len;j++)
            for (0..len) |j| {
                if (j != i) {
                    lambda[j] = lambda1[k];
                    k += 1;
                }
            }
        }
        if (temp_re == 1) {
            break;
        }
    }

    return 0;
}

//l: t*n+p
// return lambda t1
// lambda*v1+(1-lambda)*v2
// t1*n+p
// return 0 : 两线平行，有无数个最近点
// reutrn 1:只有一个最近点
pub fn line_segment_nearest_2_line(
    comptime T: type,
    l: *const [6]T,
    v1: *const [3]T,
    v2: *const [3]T,
    lambda: *T,
    t1: *T,
) i32 {
    // solve min |Ax+b|

    const b: [3]T = .{ v2[0] - l[3], v2[1] - l[4], v2[2] - l[5] };
    const A = [3][2]T{
        [2]T{ v1[0] - v2[0], -l[0] },
        [2]T{ v1[1] - v2[1], -l[1] },
        [2]T{ v1[2] - v2[2], -l[2] },
    };
    var B: [2][2]T = [2][2]T{ [2]T{ 0, 0 }, [2]T{ 0, 0 } };

    // 计算 A^T * A
    // for (int i = 0; i < 2; ++i)
    for (0..2) |i| {
        // for (int j = 0; j <= i; ++j)
        for (0..i + 1) |j| {
            // for (int k = 0; k < 3; ++k)
            for (0..3) |k| {
                B[i][j] += A[k][i] * A[k][j];
            }
            B[j][i] = B[i][j];
        }
    }
    const det: T = B[0][0] * B[1][1] - B[0][1] * B[1][0];
    // if(double_equal_zero(det))
    if (lbmath.approxEqAbs(T, det, 0, null)) {
        lambda.* = 1;
        t1.* = l[0] * (v1[0] - l[3]) +
            l[1] * (v1[1] - l[4]) +
            l[2] * (v1[2] - l[5]);
        // 因该等价下面写法
        // t1.* = -(A[0][0] * b[0] + A[1][0] * b[1] + A[2][0] * b[2] + B[0][0]) / B[0][1];
        return 0;
    }
    const B_inverse: [2][2]T = .{
        [2]T{ B[1][1], -B[0][1] },
        [2]T{ -B[1][0], B[0][0] },
    };
    const Ab: [2]T = .{ A[0][0] * b[0] + A[1][0] * b[1] + A[2][0] * b[2], A[0][1] * b[0] + A[1][1] * b[1] + A[2][1] * b[2] };

    lambda.* = -(Ab[0] * B_inverse[0][0] + Ab[1] * B_inverse[0][1]) / det;
    t1.* = -(Ab[0] * B_inverse[1][0] + Ab[1] * B_inverse[1][1]) / det;
    if (lambda.* > 1) {
        lambda.* = 1;
        // const dir = [3]T{ l[0], l[1], l[2] };
        // const base = [3]T{ l[3], l[4], l[5] };
        t1.* = l[0] * (v1[0] - l[3]) +
            l[1] * (v1[1] - l[4]) +
            l[2] * (v1[2] - l[5]);
    } else if (lambda.* < 0) {
        lambda.* = 0;
        // const dir = [3]T{ l[0], l[1], l[2] };
        // const base = [3]T{ l[3], l[4], l[5] };
        t1.* = l[0] * (v2[0] - l[3]) +
            l[1] * (v2[1] - l[4]) +
            l[2] * (v2[2] - l[5]);
    }
    return 1;
}

// @l :: t*n+p
// @poly::polygon is convex
// 求的是多边形边界和直线的交点，两个最近交点，排名最靠前的交点
// @return result[2][4]: id ,lambda,t,distance
pub fn line_closest_points_with_polygon(
    comptime T: type,
    l: *[6]T,
    poly: []const *[3]T,
    result: *[2][4]T,
) void {
    @memset(&result[0], 0);
    @memset(&result[1], 0);
    result[0][0] = -1;
    result[1][0] = -1;
    const len = poly.len;
    var num: usize = 0;
    for (0..len) |i| {
        var temp: [4]T = [4]T{ i, 0, 0, 0 };

        const next_i = (i + 1) % len;
        _ = line_segment_nearest_2_line(T, l, poly[i], poly[next_i], &temp[1], &temp[2]);

        var p1: [3]T = [3]T{
            temp[1] * poly[i][0] + (1 - temp[1]) * poly[next_i][0],
            temp[1] * poly[i][1] + (1 - temp[1]) * poly[next_i][1],
            temp[1] * poly[i][2] + (1 - temp[1]) * poly[next_i][2],
        };

        var p2: [3]T = [3]T{
            temp[2] * l[0] + l[3],
            temp[2] * l[1] + l[4],
            temp[2] * l[2] + l[5],
        };
        temp[3] = lbmath.distance_of_two_points(p1[0..3], p2[0..3]);
        if (num == 0) {
            result[0][0] = temp[0];
            result[0][1] = temp[1];
            result[0][2] = temp[2];
            result[0][3] = temp[3];
            num += 1;
            continue;
        }
        var flag: bool = false;
        for (0..num) |j| {
            if (lbmath.approxEqAbs(T, temp[3] - result[j][3], 0, null)) {
                flag = true;
                break;
            }
        }
        if (!flag) {
            var flag1: bool = false;
            for (0..num) |j| {
                if (temp[3] < result[j][3]) {
                    flag1 = true;
                    var k: usize = 1;
                    while (k > j) : (k -= 1) {
                        result[k][0] = result[k - 1][0];
                        result[k][1] = result[k - 1][1];
                        result[k][2] = result[k - 1][2];
                        result[k][3] = result[k - 1][3];
                    }
                    result[j][0] = temp[0];
                    result[j][1] = temp[1];
                    result[j][2] = temp[2];
                    result[j][3] = temp[3];
                    if (num < 2) {
                        num += 1;
                    }
                    break;
                }
            }
            if (flag1 == false and num < 2) {
                result[num][0] = temp[0];
                result[num][1] = temp[1];
                result[num][2] = temp[2];
                result[num][3] = temp[3];
                num += 1;
            }
        }
    }
}

pub fn is_point_in_line(l: anytype, p: *const [3]@TypeOf(l[0])) bool {
    comptime {
        switch (@typeInfo(@TypeOf(l))) {
            .pointer => |pp| {
                switch (pp.size) {
                    .slice => if (l.len != 6) @compileError("l.len is not 6\n"),
                    .one => if (pp.child != [6]@TypeOf(l[0])) @compileError("type erro"),
                    else => @compileError("type erro\n"),
                }
            },
            .array => if (l.len != 6) @compileError("array len isnot 6\n"),
            else => @compileError("not pointer"),
        }
    }
    const T = @TypeOf(l[0]);
    const t = l[0] * (p[0] - l[3]) + l[1] * (p[1] - l[4]) + l[2] * (p[2] - l[5]);
    const p1: [3]T = .{ t * l[0] + l[3], t * l[1] + l[4], t * l[2] + l[5] };
    const dis = lbmath.distance_of_two_points(p1[0..3], p[0..3]);

    return lbmath.approxEqAbs(T, dis, 0, null);
}

// 0 不相交
// 1 相交在点
// 2 相交为直线
pub fn line_segment_intersection_with_line(
    l: anytype,
    p1: *const [3]@TypeOf(l[0]),
    p2: *const [3]@TypeOf(l[0]),
    result: *[2]@TypeOf(l[0]),
) i32 {
    comptime {
        switch (@typeInfo(@TypeOf(l))) {
            .pointer => |pp| {
                switch (pp.size) {
                    .slice => if (l.len != 6) @compileError("l.len is not 6\n"),
                    .one => if (pp.child != [6]@TypeOf(l[0])) @compileError("type erro"),
                    else => @compileError("type erro\n"),
                }
            },
            .array => if (l.len != 6) @compileError("array len isnot 6\n"),
            else => @compileError("not pointer"),
        }
    }
    const T = @TypeOf(l[0]);
    // var l1: [6]T = [_]T{0} ** 6;
    // // 如果l1退化为点,即p1 p2同点
    // if (get_line_equation_from_two_points(p1, p2, l1[0..6]) == 0) {
    //     if (is_point_in_line(l, p1)) {
    //         result[0]=l[0]*(p1[0]-l[3])+l[1]*(p1[1]-l[4])+l[2]*(p1[2]-l[5]);
    //         // result[0][0] = p1[0];
    //         // result[0][1] = p1[1];
    //         // result[0][2] = p1[2];
    //         return 1;
    //     } else {
    //         return 0;
    //     }
    // }
    var lambda: T = 0;
    var t1: T = 0;
    const flag = line_segment_nearest_2_line(
        T,
        l,
        p1,
        p2,
        &lambda,
        &t1,
    );
    const temp1 = [3]T{
        lambda * p1[0] + (1 - lambda) * p2[0],
        lambda * p1[1] + (1 - lambda) * p2[1],
        lambda * p1[2] + (1 - lambda) * p2[2],
    };
    const temp2 = [3]T{ t1 * l[0] + l[3], t1 * l[1] + l[4], t1 * l[2] + l[5] };
    const dis = lbmath.distance_of_two_points(temp1[0..3], temp2[0..3]);
    if (lbmath.approxEqAbs(T, dis, 0, null)) {
        if (flag == 1) {
            result[0] = t1;
            return 1;
        } else {
            result[0] = t1;
            result[1] = l[0] * (p2[0] - l[3]) +
                l[1] * (p2[1] - l[4]) +
                l[2] * (p2[2] - l[5]);
            if (lbmath.approxEqAbs(T, result[0], result[1], null)) {
                return 1;
            } else {
                if (result[0] > result[1]) {
                    const temp = result[0];
                    result[0] = result[1];
                    result[1] = temp;
                }
                return 2;
            }
        }
    } else {
        return 0;
    }
}

fn insert_order_slice(comptime T: type, s: *[2]T, t: T, num: usize) u32 {
    for (0..num) |i| {
        if (lbmath.approxEqAbs(T, t, s[i], null)) {
            return 0;
        }
    }
    if (num == 0) {
        s[0] = t;
        return 1;
    } else if (num == 1) {
        s[1] = t;
        if (s[1] < s[0]) {
            const temp = s[0];
            s[0] = s[1];
            s[1] = temp;
        }
        return 1;
    }

    if (t < s[0]) {
        s[0] = t;
    } else if (t > s[1]) {
        s[1] = t;
    }

    return 0;
}

pub fn line_intersection_with_polygon(
    l: anytype,
    poly: [][3]@TypeOf(l[0]),
    result: *[2]@TypeOf(l[0]),
) i32 {
    comptime {
        switch (@typeInfo(@TypeOf(l))) {
            .pointer => |pp| {
                switch (pp.size) {
                    .slice => if (l.len != 6) @compileError("l.len is not 6\n"),
                    .one => if (pp.child != [6]@TypeOf(l[0])) @compileError("type erro"),
                    else => @compileError("type erro\n"),
                }
            },
            .array => if (l.len != 6) @compileError("array len isnot 6\n"),
            else => @compileError("not pointer"),
        }
    }
    const T = @TypeOf(l[0]);
    var num = 0;
    for (poly) |p| {
        if (is_point_in_line(l, p[0..3])) {
            const t = l[0] * (p[0] - l[3]) + l[1] * (p[1] - l[4]) + l[2] * (p[2] - l[5]);
            var flag = 0;
            for (0..num) |i| {
                if (lbmath.approxEqAbs(T, t, result[i], null)) {
                    flag = 1;
                    break;
                }
            }
            if (flag == 0) {
                result[num] = t;
                num += 1;
            }
        }
    }
    if (num == 2) {
        if (result[0] > result[1]) {
            const temp = result[0];
            result[0] = result[1];
            result[1] = temp;
        }
    }

    if (num >= 2) {
        return 2;
    }
    for (0..poly.len) |i| {
        var temp_t: [2]T = .{ 0, 0 };
        const j = (i + 1) % poly.len;
        const mark = line_segment_intersection_with_line(l, poly[i][0..3], poly[j][0..3], &temp_t);
        if (mark == 0) {
            continue;
        }
        for (0..mark) |k| {
            const step = insert_order_slice(T, result, temp_t[k], num);
            num += step;
        }
    }

    return num;
}

pub fn get_coordinate_in_another_coordinate_system(
    p: anytype,
    a1: *const [3]@TypeOf(p[0]),
    a2: *const [3]@TypeOf(p[0]),
    a3: *const [3]@TypeOf(p[0]),
    re: *[3]@TypeOf(p[0]),
) i32 {
    const T = @TypeOf(p[0]);
    var mat = lbmatrix.LBMatrix(T, 3, 3).init(0);

    // double* data=(double*)(m->data);
    mat.data[0][0] = a1[0];
    mat.data[1][0] = a1[1];
    mat.data[2][0] = a1[2];
    mat.data[0][1] = a2[0];
    mat.data[1][1] = a2[1];
    mat.data[2][1] = a2[2];
    mat.data[0][2] = a3[0];
    mat.data[1][2] = a3[1];
    mat.data[2][2] = a3[2];
    // LB_Matrix* mi=m->inverse(m);
    const mati = mat.inverse() catch return 0;

    // double * re=(double*)malloc(sizeof(double)*3);
    // memset(re,0,sizeof(double)*3);
    // for(int i=0;i<3;i++)
    for (0..3) |i| {
        re[i] = 0;
        // for(int j=0;j<3;j++)
        for (0..3) |j| {
            re[i] += mati.data[i * 3 + j] * p[j];
        }
    }
    return 1;
}

pub fn get_triangle_coordinate(
    p: anytype,
    p1: *const [3]@TypeOf(p[0]),
    p2: @TypeOf(p[0]),
    p3: @TypeOf(p[0]),
) [3]@TypeOf(p[0]) {
    const T = @TypeOf(p[0]);
    var re: [3]T = .{ 0, 0, 0 };
    const a1: [3]T = .{ p1[0] - p3[0], p1[1] - p3[1], p1[2] - p3[2] };
    const a2: [3]T = .{ p2[0] - p3[0], p2[1] - p3[1], p2[2] - p3[2] };
    const pp: [3]T = .{ p[0] - p3[0], p[1] - p3[1], p[2] - p3[2] };
    // n[3]={0};
    var n = lbmath.out_productn(&a1, &a2);
    var coord: [3]T = .{ 0, 0, 0 };
    //free(n);
    if (get_coordinate_in_another_coordinate_system(T, &pp, &a1, &a2, &n, &coord) == 0) {
        return 0;
    }
    // double *re=(double*)malloc(sizeof(double)*3);
    re[0] = coord[0];
    re[1] = coord[1];
    re[2] = 1 - re[0] - re[1];
    // free(coord);

    return 1;
}

pub fn compute_area_of_triangle(
    p1: anytype,
    p2: *const [3]@TypeOf(p1[0]),
    p3: *const [3]@TypeOf(p1[0]),
) @TypeOf(p1[0]) {
    const T = @TypeOf(p1[0]);
    const q1: [3]T = .{ p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2] };
    const q2: [3]T = .{ p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2] };
    const q: [3]T = .{ q1[1] * q2[2] - q1[2] * q2[1], -q1[0] * q2[2] + q1[2] * q2[0], q1[0] * q2[1] - q1[1] * q2[0] };
    return lbmath.safe_sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]) / 2.0;
}

//n1->n2 获得旋转变换
//
pub fn get_one_dir_trans2dir_rota_matrix(
    n1: anytype,
    n2: *const [3]@TypeOf(n1[0]),
) [3][3]@TypeOf(n1[0]) {
    // double n[3]={0};
    const T = @TypeOf(n1[0]);
    const re: [3][3]T = [3][3]T{
        [3]T{ 1, 0, 0 },
        [3]T{ 0, 1, 0 },
        [3]T{ 0, 0, 1 },
    };
    var n = lbmath.out_product(n1, n2);
    lbmath.normalize(n) catch {
        var temp_dir: [3]T = .{ 1, 0, 0 };
        n = lbmath.out_product(n1, temp_dir);
        lbmath.normalize(n) catch {
            temp_dir[1] = 1;
            n = lbmath.out_product(n1, temp_dir);
            lbmath.normalize(n) catch return re;
        };
    };
    // if(!normalize(n,3))
    // {
    //     //SAFE_FREE(n);
    //     double temp_dir[3]={1,0,0};
    //     out_productn(n1,temp_dir,n);
    //     if(!normalize(n,3))
    //     {
    //         //SAFE_FREE(n);
    //         temp_dir[1]=1;
    //         out_productn(n1,temp_dir,n);
    //         if(!normalize(n,3))
    //         {
    //             //SAFE_FREE(n);
    //             return NULL;
    //         }
    //     }
    // }

    lbmath.normalize(n1) catch re;
    lbmath.normalize(n2) catch re;

    const theta = math.acos(lbmath.inner_product(n1, n2));
    return lbmath.get_matrix_from_rotation(T, theta, n);

    //SAFE_FREE(n);
    // return re;
}
//
// 返回在poly平面的直线截取的多边形
// 右手，法向n穿过右手掌心，大拇指方向即为多边形要提取的方向
// 右手，法向n穿过右手掌心，大拇指方向即为多边形要提取的方向
pub fn get_half_section_polygon_and_line3d(
    comptime T: type,
    poly: [][]T,
    l: *const [6]T,
    allocator: std.mem.Allocator,
) ?[][3]T {
    var n = compute_plane_normal_of_jordan_curve(poly);
    lbmath.normalize(n[0..3]) catch return null;

    var y_dir = lbmath.out_product(n[0..3], l[0..3]);
    lbmath.normalize(y_dir[0..3]) catch return null;

    var temp_poly: []T = allocator.alloc(T, poly.len) catch return null;
    defer allocator.free(temp_poly);
    for (0..poly.len) |i| {
        const temp_p: [3]T = .{ poly[i][0] - l[3], poly[i][1] - l[4], poly[i][2] - l[5] };
        temp_poly[i] = y_dir[0] * temp_p[0] + y_dir[1] * temp_p[1] + y_dir[2] * temp_p[2];
    }
    var ranse1: []i32 = allocator.alloc(i32, poly.len) catch return null;
    defer allocator.free(ranse1);
    @memset(ranse1, 0);

    var ii = -1;

    // for(int i=0;i<len;i++)
    for (0..poly.len) |i| {
        if (lbmath.approxEqAbs(T, temp_poly[i], 0, null)) {
            ranse1[i] = 0;
            if (ii == -1) {
                ii = i;
            }
        } else if (temp_poly[i] > 0) {
            ranse1[i] = 1;
            ii = i;
        } else if (temp_poly[i] < 0) {
            ranse1[i] = -1;
        }
    }
    if (ii == -1) {
        return null;
    }
    const len = poly.len;
    var list = std.ArrayList([3]T).init(allocator);
    // defer list.deinit();

    for (0..poly.len) |i| {
        // printf("ranse:%d\n",ranse1[(i+ii)%len]);
        if (ranse1[(i + ii) % len] >= 0) {
            // (*re)[*len1] =(double*)malloc(sizeof(double)*3);
            // (re)[*len1][0]=poly[(i+ii)%len][0];(re)[*len1][1]=poly[(i+ii)%len ][1];(re)[*len1][2]=0;
            list.append(.{ poly[(i + ii) % len][0], poly[(i + ii) % len][1], poly[(i + ii) % len][2] });
            // *len1=*len1+1;
        }

        if ((ranse1[(i + ii) % len] > 0 and ranse1[(i + ii + 1) % len] < 0) or
            (ranse1[(i + ii) % len] < 0 and ranse1[(i + ii + 1) % len] > 0))
        {
            const lambda: T = temp_poly[(i + ii + 1) % len] / (temp_poly[(i + ii + 1) % len] - temp_poly[(i + ii) % len]);

            var pp: [3]T = .{ 0, 0, 0 };

            pp[0] = lambda * poly[(i + ii) % len][0] + (1 - lambda) * poly[(i + ii + 1) % len][0];
            pp[1] = lambda * poly[(i + ii) % len][1] + (1 - lambda) * poly[(i + ii + 1) % len][1];
            pp[2] = lambda * poly[(i + ii) % len][2] + (1 - lambda) * poly[(i + ii + 1) % len][2];

            list.append(pp);
        }
    }
    const result = list.toOwnedSlice() catch {
        list.deinit();
        return null;
    };
    return result;
}

// 提取poly在p的法向切割部分
//两个polygon顺序同向,且必须是逆时针
//

pub fn get_half_section_polygon_and_plane3d(
    comptime T: type,
    poly: [][]T,
    p: *const [4]T,
    allocator: std.mesh.Allocator,
) ?[][3]T {
    var temp_poly: []T = allocator.alloc(T, poly.len) catch return null;
    defer allocator.free(temp_poly);

    for (0..poly.len) |i| {
        // 这是因为-p[0]*p[3],-p[1]*p[3],-p[2]*p[3] 是过平面的点
        const temp_p: [3]T = .{ poly[i][0] + p[0] * p[3], poly[i][1] + p[1] * p[3], poly[i][2] + p[2] * p[3] };
        temp_poly[i] = p[0] * temp_p[0] + p[1] * temp_p[1] + p[2] * temp_p[2];
    }
    var ranse1: []i32 = allocator.alloc(i32, poly.len) catch return null;
    defer allocator.free(ranse1);
    @memset(ranse1, 0);

    var ii = -1;

    // for(int i=0;i<len;i++)
    for (0..poly.len) |i| {
        if (lbmath.approxEqAbs(T, temp_poly[i], 0, null)) {
            if (ii == -1) {
                ii = i;
            }
            ranse1[i] = 0;
        } else if (temp_poly[i] > 0) {
            ranse1[i] = 1;
            ii = i;
        } else if (temp_poly[i] < 0) {
            ranse1[i] = -1;
        }
    }
    if (ii == -1) {
        return null;
    }
    const len = poly.len;
    var list = std.ArrayList([3]T).init(allocator);
    // defer list.deinit();

    for (0..poly.len) |i| {
        // printf("ranse:%d\n",ranse1[(i+ii)%len]);
        if (ranse1[(i + ii) % len] >= 0) {
            list.append(.{ poly[(i + ii) % len][0], poly[(i + ii) % len][1], poly[(i + ii) % len][2] });
        }

        if ((ranse1[(i + ii) % len] > 0 and ranse1[(i + ii + 1) % len] < 0) or
            (ranse1[(i + ii) % len] < 0 and ranse1[(i + ii + 1) % len] > 0))
        {
            const lambda: T = temp_poly[(i + ii + 1) % len] / (temp_poly[(i + ii + 1) % len] - temp_poly[(i + ii) % len]);

            var pp: [3]T = .{ 0, 0, 0 };

            pp[0] = lambda * poly[(i + ii) % len][0] + (1 - lambda) * poly[(i + ii + 1) % len][0];
            pp[1] = lambda * poly[(i + ii) % len][1] + (1 - lambda) * poly[(i + ii + 1) % len][1];
            pp[2] = lambda * poly[(i + ii) % len][2] + (1 - lambda) * poly[(i + ii + 1) % len][2];

            list.append(pp);
        }
    }
    const result = list.toOwnedSlice() catch {
        list.deinit();
        return null;
    };
    return result;
}

// 不管 poly1 poly2 是不是在一个平面，返回的是相交区域，可以是null,点，线，面
// 保证poly1和poly2是凸的

pub fn get_intersections_from_two_polygons(
    comptime T: type,
    poly1: [][]T,
    poly2: [][]T,
    allocator: std.mem.Allocator,
) ?[][3]T {
    // var len:usize=0;
    var p1: [4]T = .{ 0, 0, 0, 0 };
    get_plane_equation_of_polygon(poly1, &p1);

    var p2: [4]T = .{ 0, 0, 0, 0 };
    get_plane_equation_of_polygon(poly2, &p2);
    // double n[3]={0};
    var n = lbmath.out_product(p1[0..3], p2[0..3]);
    lbmath.normalize(n[0..3]) catch {
        if (lbmath.approxEqAbs(T, lbmath.inner_product(p1[0..3], poly2[0][0..3]) + p1[3], 0, null)) {
            return null;
        }

        const step: i32 = if (lbmath.inner_product(p1[0..3], p2[0..3]) > 0) 1 else -1;
        // int step=0;
        // inner_product(p1,p2,3)>0?(step=1):(step=-1);
        //
        const len1 = poly1.len;
        const len2 = poly2.len;
        var re = allocator.alloc([3]T, len2) catch return null;
        for (0..len2) |i| {
            re[i][0] = poly2[i][0];
            re[i][1] = poly2[i][1];
            re[i][2] = poly2[i][2];
        }

        var i = 0;
        for (0..len1) |_| {
            const j = (i + step + len1) % len1;
            var l: [6]T = [_]T{0} ** 6;
            get_line_equation_from_two_points(poly1[i], poly1[j], &l);

            const temp_re = get_half_section_polygon_and_line3d(re, l, allocator) orelse {
                allocator.free(re);
                return null;
            };
            allocator.free(re);
            re = temp_re;
            i = j;
        }
        return re;
    };
    // if( !normalize(n,3) )

    // *len=0;
    var l: [6]T = [_]T{0} ** 6;
    get_line_equation_from_2planes(p1[0..4], p2[0..4], l[0..6]);
    var tempt1: [2]T = .{ 0, 0 };
    const num1 = line_intersection_with_polygon(
        l[0..6],
        poly1,
        tempt1[0..2],
    );
    if (num1 <= 0) {
        return null;
    } else if (num1 == 1) {
        tempt1[1] = tempt1[0];
    }
    var tempt2: [2]T = .{ 0, 0 };
    const num2 = line_intersection_with_polygon(
        l[0..6],
        poly2,
        tempt2[0..2],
    );
    if (num2 <= 0) {
        return null;
    } else if (num2 == 1) {
        tempt2[1] = tempt2[0];
    }
    if (tempt1[0] <= tempt2[1] and tempt1[1] >= tempt2[0]) {
        // var list=std.ArrayList()
        const t1 = if (tempt1[0] < tempt2[0]) tempt2[0] else tempt1[0];
        const t2 = if (tempt1[1] < tempt2[1]) tempt1[1] else tempt2[1];
        if (lbmath.approxEqAbs(T, t1, t2, null)) {
            const re = allocator.alloc([3]T, 1);
            re[0][0] = t1 * l[0] + l[3];
            re[0][1] = t1 * l[1] + l[4];
            re[0][2] = t1 * l[2] + l[5];
            return re;
        }
        const re = allocator.alloc([3]T, 2);
        re[0][0] = t1 * l[0] + l[3];
        re[0][1] = t1 * l[1] + l[4];
        re[0][2] = t1 * l[2] + l[5];
        re[1][0] = t2 * l[0] + l[3];
        re[1][1] = t2 * l[1] + l[4];
        re[1][2] = t2 * l[2] + l[5];
        return re;
    }

    return null;
}
